#include <XnCppWrapper.h>
#include "MidiPlayer.h"
#include <vector>
#include <gl/GLU.h>
#include <math.h>

#ifdef _DEBUG
#define new DEBUG_NEW
#endif


#define GL_WIN_SIZE_X	1366
#define GL_WIN_SIZE_Y	768



MidiPlayer::MidiPlayer()
{
	on = false;
	playing = false;
	key = C;
	prevKey = None;
	prevKeyValid = false;
	replay = true;
	mChord.clear();
	inst = 57;//49;//96
	mVol = 63;
	prevZ = -1.;
	lastNote = time(NULL);

	RECT rc;
	GetWindowRect(GetDesktopWindow(), &rc);
	mRC = rc;
	int x = GetDif(rc.left, rc.right);
	int y = GetDif(rc.top, rc.bottom);
	cenX = x/2;
	cenY = y/2;

	mCircle.SetRECT(rc, x, y);
	int minDim = min(x, y);
	minRadius = (2*minDim) / 5; 
	maxRadius = minDim - 50;
	mCircle.SetRad((int)minRadius/2, (int)maxRadius/2);

	OpenMidi();
}

MidiPlayer::~MidiPlayer()
{
	AllNotesOff(true); //Turn off all notes (necessary?)
	if (on) CloseMidi(); //Then turn off midi
	mChord.clear();
	//mPrevChord.clear();
	mPoints.clear();
}

void MidiPlayer::OpenMidi()
{
	if (on)
		return;

	MMRESULT hr;

    // How many devices are there?
    UINT num = midiOutGetNumDevs();
    MIDIOUTCAPS caps;
    bool found = false;
    UINT devId = 0;

    // Iterate over the devices and get their capabilites
    for(UINT n=0;  n<num;  n++)
    {

        hr = midiOutGetDevCaps(n, &caps, sizeof(caps));
        if(hr == MMSYSERR_NOERROR)
        {
            // Mine found: szPname	0x0031f5ec "Microsoft GS Wavetable Synth"	wchar_t [32]
            found = true;
            devId = n;
            break;
        }
    }

    if(!found)
    {
        printf("No suitable MIDI device found");
        return;
    }

    // Try to open for output
    hr = midiOutOpen(&mHMidiOut, devId, NULL, NULL, CALLBACK_NULL);
    
    // Create an output buffer
    mBuffer.resize(1024);       // No more than 65535, BTW

    // Prepare the header
    mHeader.lpData = (LPSTR)&mBuffer[0];
    mHeader.dwBufferLength = mBuffer.size();
    mHeader.dwFlags = 0;

    hr = midiOutPrepareHeader(mHMidiOut, &mHeader, sizeof(mHeader));

    on = true;

    // Try selecting an instrument
    SendMidiMessage(0xc0 + 0, inst);
}

void MidiPlayer::CloseMidi()
{
	MMRESULT hr;
	hr = midiOutClose(mHMidiOut);
	on = false;
}


void MidiPlayer::CreateChord()
{
	if (mChord.size() == 0 || mChord[0][0].mChord != key)
	{
		//If there aren't currently chords, or if the most recent one isn't in the current key, 
		//    make a new Chord
		std::vector<Note> newChord;
		//mPrevChord = mChord;
		//	A=3, B=5, C=0, D=2, E=4, F=11, G=1, As=10, Cs=7, Ds=9, Fs=6, Gs=8, Ab=8, Bb=10, Db=7, Eb=9, Gb=6
		switch(key)
		{
			//Push back two octaves of notes
			case C: { //CEG
				Keys myarray [] = { C1, E1, G1, C2, E2, G2, C3 }; //48, 52, 55, 60, 64, 67, 72 };
				for (int i=0; i<7; i++)
					newChord.push_back(Note(myarray[i], C, mVol, true)); }  
				break;
			case G: { //GBD
				Keys myarray [] = { G1, B1, D1, G2, B2, D2, G3 }; //55, 59, 62, 67, 71, 74, 79 };
				for (int i=0; i<7; i++)
					newChord.push_back(Note(myarray[i], G, mVol, true)); }  
				break;
			case D: { //DGbA
				Keys myarray [] = { D1, Gb1, A1, D2, Gb2, A2, D3 }; //50, 53, 57, 62, 65, 69, 74 };
				for (int i=0; i<7; i++)
					newChord.push_back(Note(myarray[i], D, mVol, true)); }  
				break;
			case A: { //ADbE
				Keys myarray [] = { A1, Db1, E1, A2, Db2, E2, A3 }; //57, 61, 64, 69, 73, 76, 81 };
				for (int i=0; i<7; i++)
					newChord.push_back(Note(myarray[i], A, mVol, true)); }
				break;
			case E: { //EAbB
				Keys myarray [] = { E1, Ab1, B1, E2, Ab2, B2, E3 }; //52, 56, 59, 64, 68, 71, 76 };
				for (int i=0; i<7; i++)
					newChord.push_back(Note(myarray[i], E, mVol, true)); }  
				break;
			case B: { //BEbGb
				Keys myarray [] = { B1, Eb1, Gb1, B2, Eb2, Gb2, B3 }; //59, 63, 66, 71, 75, 78, 83 };
				for (int i=0; i<7; i++)
					newChord.push_back(Note(myarray[i], B, mVol, true)); }
				break;
			case Gb: { //Also F#... GbBbDb
				Keys myarray [] = { Gb1, Bb1, Db1, Gb2, Bb2, Db2, Gb3 }; //54, 58, 61, 66, 70, 73, 78 };
				for (int i=0; i<7; i++)
					newChord.push_back(Note(myarray[i], Gb, mVol, true)); }  
				break;
			case Db: { //Also C#... DbFAb
				Keys myarray [] = { Db1, F1, Ab1, Db2, F2, Ab2, Db3 }; //49, 53, 56, 61, 65, 68, 73 };
				for (int i=0; i<7; i++)
					newChord.push_back(Note(myarray[i], Db, mVol, true)); }  
				break;
			case Ab: { //Also G#... AbCEb
				Keys myarray [] = { Ab1, C1, Eb1, Ab2, C2, Eb2, Ab3 }; //56, 60, 63, 68, 72, 75, 80 };
				for (int i=0; i<7; i++)
					newChord.push_back(Note(myarray[i], Ab, mVol, true)); }  
				break;
			case Eb: { //Also D#... EbGBb
				Keys myarray [] = { Eb1, G1, Bb1, Eb2, G2, Bb2, Eb3 }; //51, 55, 58, 63, 67, 70, 75 };
				for (int i=0; i<7; i++)
					newChord.push_back(Note(myarray[i], Eb, mVol, true)); }    
				break;
			case Bb: { //Also A#... BbDF
				Keys myarray [] = { Bb1, D1, F1, Bb2, D2, F2, Bb3 }; //58, 62, 65, 70, 74, 77, 82 };
				for (int i=0; i<7; i++)
					newChord.push_back(Note(myarray[i], Bb, mVol, true)); }  
				break;
			case F: { //FAC
				Keys myarray [] = { F1, A1, C1, F2, A2, C2, F3 }; //53, 57, 60, 65, 69, 72, 77 };
				for (int i=0; i<7; i++)
					newChord.push_back(Note(myarray[i], F, mVol, true)); }  
				break;
			default: {
				//anything?
				break; }
		}

		if (mChord.size() == 0)
			mChord.push_back(newChord);
		else mChord.insert(mChord.begin(), newChord); //Put the most important/recent chord at the beginning

		if (mChord.size() > 3)
			mChord.pop_back(); //Keep the max chord # at 3

		//Start fading out the other notes... 
		for (int i=1; i<(int)mChord.size(); i++)
		{
			for (int j=0; j<(int)mChord[i].size(); j++)
				mChord[i][j].mFade = true;
		}
		//if (mPrevChord != mChord)
			//AllNotesOff(false);
	}
	else //If there are chords, but the most recent one is the same key... Don't do anything?
		true;
	
	Fade();
}




void MidiPlayer::NoteOn(int channel, int note, int velocity)
{
    SendMidiMessage(0x90 + channel, note, velocity);
}

void MidiPlayer::NoteOff(int channel, int note)
{
    SendMidiMessage(0x80 + channel, note, 127);
}


//! This function sends the normal short midi messages such as 
//! Note On, Note Off, etc. to the device we have opened.
//! See the messages at: http://www.midi.org/techspecs/midimessages.php
void MidiPlayer::SendMidiMessage(int b0, int b1, int b2)
{
    if(!on)
        return;

    // Convenient UNION to make it easier to make the note message
    union { 
        DWORD dwData; 
        BYTE bData[4]; 
    } u; 
 
    // Construct the MIDI message. 
 
    u.bData[0] = b0;   // MIDI status byte 
    u.bData[1] = b1;   // first MIDI data byte  often note number (C4)
    u.bData[2] = b2;   // second MIDI data byte  often velocity (max)
    u.bData[3] = 0; 
 
    // Send the message. 
    midiOutShortMsg(mHMidiOut, u.dwData); 
}



void MidiPlayer::AllNotesOn()
{
	/*if (playing) //Might be redundant.....? 
	{
		AllNotesOff(false); //Or start fade...?
		MMRESULT hr;
		hr = midiOutClose(mHMidiOut);
		OpenMidi();
	}*/
	if (mChord.size() > 0)
	{
		for (int i=0; i<(int)mChord[0].size(); i++)
			NoteOn(0, (int)mChord[0][i].mKey, (int)mChord[0][i].mVol);
	}

	printf("Current chord: %s \n", getKeyName(key) );

	playing = true;
	lastNote = time(NULL);
	if (prevKey == None) prevKey = key;
}


void MidiPlayer::AllNotesOff(bool All)
{
	for (int i=1; i<(int)mChord.size(); i++)
	{
		for (int j=0; j<(int)mChord[i].size(); j++)
			NoteOff(0, (int)mChord[i][j].mKey);
	}

	if (All)
	{
		if (mChord.size() > 0)
		{
			for (int i=0; i<(int)mChord[0].size(); i++)
				NoteOff(0, (int)mChord[0][i].mKey);
		}
		playing = false; //Only turn off playing if all notes are off
	}
}

void MidiPlayer::LowerVolume()
{
	switch(mVol)
	{
	case 127:
		mVol = 115;
		break;
	case 115:
		mVol = 100;
		break;
	case 100:
		mVol = 87;
		break;
	case 87:
		mVol = 63;
		break;
	case 63:
		mVol = 31;
		break;
	case 31:
		mVol = 15;
		break;
	case 15:
		mVol = 0;
		break;
	default:
		//If, for w/e reason, it's a weird #... just turn it off
		mVol = 0;
		break;
	}
	/*AllNotesOff(false); //Let the playing function actually change volume...
	if (mVol > 0)
		AllNotesOn();*/
	if (mChord.size() > 0)//for (int i=0; i<(int)mChord.size(); i++)
	{
		for (int j=0; j<(int)mChord[0].size(); j++)
			mChord[0][j].mVol = mVol;
	}

}


void MidiPlayer::RaiseVolume()
{
	switch(mVol)
	{ 
	case 127:
		//Do nothing, no louder!
		break;
	case 115:
		mVol = 127;
		break;
	case 100:
		mVol = 115;
		break;
	case 87:
		mVol = 100;
		break;
	case 63:
		mVol = 87;
		break;
	case 31:
		mVol = 63;
		break;
	case 15:
		mVol = 31;
		break;
	case 0:
		mVol = 15;
		break;
	default:
		//If, for w/e reason, it's a weird #... just turn it off
		mVol = 0;
		break;
	}
	/*AllNotesOff(false);
	if (mVol > 0)
		AllNotesOn();*/
	if (mChord.size() > 0)// Just the first chord, assume the others are fading or gone
	{
		for (int j=0; j<(int)mChord[0].size(); j++)
			mChord[0][j].mVol = mVol;
	}
}


void MidiPlayer::AddPoint(XnPoint3D point)
{
	/*XnPoint3D noZ;
	noZ.X = point.X;
	noZ.Y = point.Y;
	noZ.Z = 0;*/
	mPoints.push_back(point); //Maybe we want z in future, why gimp ourselves
	if ((int)mPoints.size() > 20) //Don't want to just keep adding points... yikes!
		mPoints.erase(mPoints.begin());
}

double Dist(int x1, int y1, int x2, int y2)
{
	double x = (double)x2 - x1;
	double y = (double)y2 - y1;
	return sqrt( x*x + y*y );
}

std::vector<double> avg(std::vector<XnPoint3D> pts)
{
	//get the avg of all the x's, y's, z's...
	std::vector<double> total (3, 0.);
	double tX = 0.;
	double tY = 0.;
	double tZ = 0.;
	int numZ = 0;
	for (int i=0; i<(int)pts.size(); i++)
	{
		tX += pts[i].X;
		tY += pts[i].Y;
		if (pts[i].Z > 0) //Only accept positive Z values
		{
			tZ += pts[i].Z;
			numZ++;
		}
	}

	tX /= (double)pts.size();
	tY /= (double)pts.size();
	tZ /= (double)numZ;
	total[0] = tX;
	total[1] = tY;
	total[2] = tZ;

	return total;
}

/*
Evaluate the points given by HandViewer:
   Find the vector that best fits all the points, origin is center of screen
   If vector is within minRadius and maxRadius, find closest vector to determine chord to play
*/
void MidiPlayer::EvalPoints()
{	
	//First, determine if it's within min and max radii
	if ((int)mPoints.size() > 0)
	{
		std::vector<double> avgs = avg(mPoints);
		double xP = avgs[0];
		double yP = avgs[1];
		double zP = avgs[2];
		if (prevZ < 0 && zP > 0)
			prevZ = zP;
		double d = Dist(cenX, cenY, (int)xP, (int)yP);
		if (minRadius/2 <= d && d <= maxRadius/2)
		{
			double rightX = cenX + d; //setting 0 degrees to point right
			double rightY = cenY;

			//Second, find angle (radians) that represents the current point
			double xd = rightX - xP;
			double yd = rightY - yP;
			//Check if xd or yd is 0!
			if (xd == 0.) xd = 0.0000001;
			if (yd == 0.) yd = 0.0000001;
			double radians = 2 * atan(yd / xd); //Times 2 to make it between 0 and 2pi

			//printf("Currrent z: %d", zP);
			if (GetDif(zP, prevZ) > 100.)
			{
				//printf("%d", zP);
				if (zP > prevZ) //farther away, quieter
					LowerVolume();
				else RaiseVolume();
				prevZ = zP;
				replay = true;
			}
			SetBestChord(radians);
			CreateChord();
			if ((prevKey != key || replay))// && CheckTime())
			{
				AllNotesOff(true);//Should be replaced with FadePrevious
				AllNotesOn();
				prevKey = key;
				if (replay) replay = false;
			}
		}
		else if (d > maxRadius/2)
			AllNotesOff(true);
		
		if (CheckInstr(xP, yP, zP))
		{
			replay = true; //Let the function take care of switching the instrument
			CloseMidi();
			OpenMidi();
		}
	}
}

bool MidiPlayer::CheckTime()
{
	//Return true if enough time has pass since previous on note
	time_t current = time(NULL);
	if (difftime(current, lastNote) >= 1.) //60bpm, for now
	{
		lastNote = current;
		return true;
	}
	else return false;
}

void DrawRects(RECT mRC)
{
	//
	/*
	if (hX > x && hX < x*3)
	{
		if (hY > y && hY < y*3)
		{
			return true;
		}
	}*/
	double xOff = 30.;
	double x = mRC.left + xOff;
	double height = GetDif(mRC.bottom, mRC.top);
	double y = height/4.;
	glColor3d(1., 1., 1.);
	//Piano
	glBegin(GL_LINE_LOOP);
	glVertex3d(x-10., y+10., 0.);
	glVertex3d(x+70., y+10., 0.);
	glVertex3d(x+70., y-25., 0.);
	glVertex3d(x-10., y-25., 0.);
	glEnd();

	//Organ
	y = height/2.f;
	glBegin(GL_LINE_LOOP);
	glVertex3d(x-10., y+10., 0.);
	glVertex3d(x+70., y+10., 0.);
	glVertex3d(x+70., y-25., 0.);
	glVertex3d(x-10., y-25., 0.);
	glEnd();

	//Strings
	y = 3.f*height/4.f;
	glBegin(GL_LINE_LOOP);
	glVertex3d(x-10., y+10., 0.);
	glVertex3d(x+70., y+10., 0.);
	glVertex3d(x+70., y-25., 0.);
	glVertex3d(x-10., y-25., 0.);
	glEnd();

	//Trumpet
	x = mRC.right - xOff*4;
	y = height/4.f;
	glBegin(GL_LINE_LOOP);
	glVertex3d(x-10., y+10., 0.);
	glVertex3d(x+95., y+10., 0.);
	glVertex3d(x+95., y-25., 0.);
	glVertex3d(x-10., y-25., 0.);
	glEnd();

	//Alto Sax
	y = height/2.f;
	glBegin(GL_LINE_LOOP);
	glVertex3d(x-10., y+10., 0.);
	glVertex3d(x+95., y+10., 0.);
	glVertex3d(x+95., y-25., 0.);
	glVertex3d(x-10., y-25., 0.);
	glEnd();

	//Ocarina
	y = 3.f*height/4.f;
	glBegin(GL_LINE_LOOP);
	glVertex3d(x-10., y+10., 0.);
	glVertex3d(x+95., y+10., 0.);
	glVertex3d(x+95., y-25., 0.);
	glVertex3d(x-10., y-25., 0.);
	glEnd();


}


/*
Draw the chord information on the screen so the user can see which area belongs to which chord
*/
void MidiPlayer::DrawOverlay()
{
	mCircle.Draw(key, true);
	DrawRects(mRC);
	/*glColor3d(0., 0., 1.);
	glBegin(GL_LINE_LOOP);
		glVertex2i(3*/
}


void MidiPlayer::SetBestChord(double curRad)
{
	double prev = 30.; //radians will be less than this
	double rad = 0.;
	int best = 0;
	while (curRad < 0)
	{
		curRad += 6.28318531; //# of radians in a circle
		double diffr = GetDif(6.28318531, curRad);
		if (GetDif(diffr, 0.) < 0.01) curRad = 0.001;
		if (GetDif(diffr, 6.28318531) < 0.01) curRad = 6.27;
	}
	while (curRad > 6.28318531)
	{
		curRad -= 6.28318531; 
		double diffr = GetDif(6.28318531, curRad);
		if (GetDif(diffr, 0.) < 0.01) curRad = 0.001;
		if (GetDif(diffr, 6.28318531) < 0.01) curRad = 6.27;
	}

	for (int i=0; i<13; i++)
	{
		rad = i * 0.5235988; //Radians in 30 degrees
		double curDif = 0.;
		if (curRad > rad) curDif = curRad - rad;
		else curDif = rad - curRad;
		if (curDif < prev)
		{
			//i is better approximation
			best = i;
			prev = curDif;
		}
	}

	if (best == 12) //Allows for overflow, in case the angle is just under 360 degrees
		best = 0;

	int bDif = best - 3;
	if (bDif < 0)
		bDif = 12 + bDif;
	best = (bDif) % 12; //Reverse the direction... sigh
	//if (prevKey != static_cast<Chords>(best) && key != static_cast<Chords>(best))
		//prevKey = key; //Update it to most recent different chord
	key = static_cast<Chords>(best);
	if (prevKey != key)
	{
		mCircle.Draw(key, false);
	}
}


void MidiPlayer::Fade()
{
	//Work on fading the others out...? Best to call this somewhere where it always happens... draw?
	for (int i=1; i<(int)mChord.size(); i++)
	{
		for (int j=0; j<(int)mChord[i].size(); j++)
			mChord[i][j].mVol -= 2; //Test for the best rate... ignoring that it's logarithmic for now...
	}
	
	//Now check last two chords (the only two possible besides the main one)... if their volume is less than... 10? delete them
	if ((int)mChord.size() > 2)
	{
		if (mChord[1][0].mVol <= 10) //If the second will be removed, might as well remove the third
		{
			mChord.erase(mChord.begin() + 2);
			mChord.erase(mChord.begin() + 1);
		}
		else if (mChord[2][0].mVol <= 10)
			mChord.erase(mChord.begin() + 2);
	}
	if ((int)mChord.size() > 1)
	{
		if (mChord[1][0].mVol <= 10)
			mChord.erase(mChord.begin() + 1);
	}
}


bool InRect(double x, double y, double hX, double hY)
{
	/*double x = mRC.left + 30.;
	double y = GetDif(mRC.bottom, mRC.top)/4.;

	glBegin(GL_LINE_LOOP);
	glVertex3d(x-10., y+10., 0.);
	glVertex3d(x+70., y+10., 0.);
	glVertex3d(x+70., y-25., 0.);
	glVertex3d(x-10., y-25., 0.);
	glEnd();*/

	if (hX >= x-10 && hX <= x+70)
	{
		if (hY >= y-25 && hY <= y+10)
		{
			return true;
		}
	}
	return false;
}

bool MidiPlayer::CheckInstr(double aX, double aY, double aZ)
{
	//aX, aY, aZ are the average of the past <20 points... determine if the x,y is in the instrument squares
	RECT rc = mRC;
	//Draw Instruments
	float xOff = 30.f;
	float width = (float)GetDif((double)rc.left, (double)rc.right);
	float height = (float)GetDif((double)rc.top, (double)rc.bottom);

	//Piano
	//printw(rc.left + xOff, height/4.f, 0.f, "Piano");

	if (InRect(rc.left + xOff, height/4.f, aX, aY))
		inst = 0;

	//Organ
	else if (InRect(rc.left + xOff, height/2.f, aX, aY))
		inst = 17;

	//Strings
	else if (InRect(rc.left + xOff, 3.f*height/4.f, aX, aY))
		inst = 49;

	//Trumpet
	else if (InRect(rc.right - xOff*4, height/4.f, aX, aY))
		inst = 57;

	//Alto Sax
	else if (InRect(rc.right - xOff*4, height/2.f, aX, aY))
		inst = 66;

	//Ocarina
	else if (InRect(rc.right - xOff*4, 3.f*height/4.f, aX, aY))
		inst = 80;

	else return false;

	return true;
}

